/**
 * \file
 *
 * \brief Empty user application template
 *
 */

/*
 * Include header files for all drivers that have been imported from
 * AVR Software Framework (ASF).
 */
#include <asf.h>
#include "conf_board.h"
#include "conf_pdca.h"
#include "imu_sensors.h"
#include "conf_startup.h"
#include "scheduler.h"
#include "ekf.h"
#include "usart1_pdca.h"
#include <math.h>

int main (void)
{
	struct marg_sensors_t marg_s; 
	unsigned long dt;
	unsigned long pidDT;
	unsigned int temp;
	
	/*pitch PID*/
	float perror = 0, pderivative = 0, pintegral = 0,ppreverror = 0, poutput = 0;
	
	/*Roll PID*/
	float rerror = 0, rderivative = 0, rintegral = 0,rpreverror = 0, routput = 0;
	
	/*pitch PID constants*/
	float ker = 0,kir = 0,kdr = 0;
	float kep = 0,kip = 0,kdp = 0;
	float key = 0,kiy = 0,kdy = 0;
	
	/*User thrust*/
	unsigned int ut = 0;
	
	/*User Angle*/
	int upa = 0;
	int ura = 0;
	int uya = 0;
	
	/*quaternion stuff*/
	float q0;
	float q1;
	float q2;
	float q3;
	float r;
	float p;
	float y;
	
	
	startup();
	
	marg_s.sensor_t = ACCELEROMETER;
	init_sensors(&marg_s);

	marg_s.sensor_t = ITG3200;
	init_sensors(&marg_s);

	marg_s.sensor_t = MAGNETOMETER;
	init_sensors(&marg_s);
	
	gpio_tgl_gpio_pin(AVR32_PIN_PB09);
	
	
	/*initizlie the EKF vars etc*/
	ekf_init(&marg_s);
	
	/*populate the USART1 PDCA*/
	USART1_pdca_MARG(&marg_s);
	
	print_serial_sensor(&marg_s, 10);
	
	/*for scheduling - ekf runs at 500hz
	  acc/gyro collects data at 500hz interpolated
	  mag collects data at 100hz.
	 */
	sensor_scheduler_init(&marg_s);
	print_scheduler_init(&marg_s);
	start_schedulers();
	
	/*let everything normalize*/
	delay_ms(500);
	
	marg_s.previousUpdate = millis;
	pidDT = millis;
	while(1)
	{	
		if(millis - pidDT >=5)
		{
			/*Process quaternion to degrees*/
			q0 = marg_s.state_q0;
			q1 = marg_s.state_q1;
			q2 = marg_s.state_q2;
			q3 = marg_s.state_q3;
			r = marg_s.r = atan( 2*(q0 * q1 + q2 * q3)/(1-2*(q1*q1 + q2 * q2))) * (RAD2DEG);
			p = marg_s.p = asin(2 * (q0*q2-q3*q1)) * (RAD2DEG);
			y = marg_s.y =  atan( 2*(q0 * q3 + q1 * q2)/(1-2*(q2*q2 + q3 * q3))) * (RAD2DEG);
						
			/*PID control area, ROLL*/		
			rerror = ura - r;
			rintegral = rintegral + rerror*((float)(millis - pidDT)/1000.00);
			rderivative = (rerror - rpreverror)/((float)(millis - pidDT)/1000.00);
			routput = ker*rerror + kir*rintegral + kdr*rderivative;
			rpreverror = rerror;
			
			
			/*PID control area, PITCH*/		
			perror = upa - p;
			pintegral = pintegral + perror*((float)(millis - pidDT)/1000.00);
			pderivative = (perror - ppreverror)/((float)(millis - pidDT)/1000.00);
			poutput = kep*perror + kip*pintegral + kdp*pderivative;
			ppreverror = perror;		
			
				
		
			if(ut < 500 && poutput < 100  && routput < 100)
			{
				pwm_async_update_channel_fast(0,1100 + (unsigned int)ut*2 - (int)poutput);
				pwm_async_update_channel_fast(1,1100 + (unsigned int)ut*2 + (int)poutput);
					
				pwm_async_update_channel_fast(2,1100 + (unsigned int)ut*2 - (int)routput);
				pwm_async_update_channel_fast(3,1100 + (unsigned int)ut*2 + (int)routput);	
			}		
			pidDT = millis;	
			
			/*PID Tune parser*/
			if(pdca_get_load_size(USART1_PDCA_CHANNEL_IN) <= 1008)
			{
				ut = (unsigned int)inbuffer[0];
				ura = (int)inbuffer[1];
				upa = (int)inbuffer[2];
				uya = (int)inbuffer[3];
				
				switch(temp = (unsigned int)inbuffer[4])
				{
					case 0://x
						ker = (inbuffer[5] | (inbuffer[6] << 8) | (inbuffer[7] << 16) | (inbuffer[8]<<24))/1000.00;
						kir = (inbuffer[9] | (inbuffer[10] << 8) | (inbuffer[11] << 16) | (inbuffer[12]<<24))/1000.00;
						kdr = (inbuffer[13] | (inbuffer[14] << 8) | (inbuffer[15] << 16) | (inbuffer[16]<<24))/1000.00;
					break;
					case 1://y
						kep = (inbuffer[5] | (inbuffer[6] << 8) | (inbuffer[7] << 16) | (inbuffer[8]<<24))/1000.00;
						kip = (inbuffer[9] | (inbuffer[10] << 8) | (inbuffer[11] << 16) | (inbuffer[12]<<24))/1000.00;
						kdp = (inbuffer[13] | (inbuffer[14] << 8) | (inbuffer[15] << 16) | (inbuffer[16]<<24))/1000.00;
					break;
					case 2://z
						key = (inbuffer[5] | (inbuffer[6] << 8) | (inbuffer[7] << 16) | (inbuffer[8]<<24))/1000.00;
						kiy = (inbuffer[9] | (inbuffer[10] << 8) | (inbuffer[11] << 16) | (inbuffer[12]<<24))/1000.00;
						kdy = (inbuffer[13] | (inbuffer[14] << 8) | (inbuffer[15] << 16) | (inbuffer[16]<<24))/1000.00;
					break;
					default:
					break;
					
				}

				printf("%f %f %f\n", r,p,y);
				//printf("%f %f %f %f | %f %f %f %f | %f %f\n",perror,pintegral,pderivative,poutput,rerror,rderivative,rintegral,routput, r, p);
				//printf("out = %f, p = %f i = %f d = %f t = %lu\n",poutput,perror,pintegral, marg_s.G_y,(millis - pidDT));
				pdca_load_channel(USART1_PDCA_CHANNEL_IN,inbuffer,1024);
			
			}	
		}	
			
		/*process quaternions*/	
		if(marg_s.G_Set)
		{		
			/*gyroscope*/
			dt = millis - marg_s.previousUpdate;
			marg_s.previousUpdate = millis;
			
			marg_s.G_x = (marg_s.Gyro_buf)[0] | ((marg_s.Gyro_buf)[1] << 8);
			marg_s.G_y = (marg_s.Gyro_buf)[2] | ((marg_s.Gyro_buf)[3] << 8);
			marg_s.G_z = (marg_s.Gyro_buf)[4] | ((marg_s.Gyro_buf)[5] << 8);
			marg_s.G_Set = false;
			process_gyro((float)marg_s.G_x,(float)marg_s.G_y,(float) marg_s.G_z,(float)dt/1000.00);
			
				
		}	
		if(marg_s.A_Set)
		{	
			long b = millis;
			marg_s.A_x = (marg_s.Acc_buf)[0] | ((marg_s.Acc_buf)[1] << 8);
			marg_s.A_y = (marg_s.Acc_buf)[2] | ((marg_s.Acc_buf)[3] << 8);
			marg_s.A_z = (marg_s.Acc_buf)[4] | ((marg_s.Acc_buf)[5] << 8);
			marg_s.A_Set = false;
			process_acc((float)marg_s.A_x,(float)marg_s.A_y,(float) marg_s.A_z);

		}	
		
		if(marg_s.M_Set)
		{	
			marg_s.M_x = (marg_s.Mag_buf)[1] | ((marg_s.Mag_buf)[0] << 8);
			marg_s.M_y = (marg_s.Mag_buf)[3] | ((marg_s.Mag_buf)[2] << 8);
			marg_s.M_z = (marg_s.Mag_buf)[5] | ((marg_s.Mag_buf)[4] << 8);
			marg_s.M_Set = false;
			//process_mag((float)marg_s.M_x,(float)marg_s.M_y,(float)marg_s.M_z);
		}	
		
		gpio_tgl_gpio_pin(AVR32_PIN_PB09);
				
		//
		
		//printf("%f %f %f %f\n",marg_s.state_q0, marg_s.state_q1, marg_s.state_q2,marg_s.state_q3);
		//printf("%f %f %f\n",p, r, y);
		//printf("AR: %d %d %d\n",marg_s.G_x, marg_s.G_y, marg_s.G_z);
		//printf("%d %d %d %d %d %d %d %d %d\n",marg_s.A_x, marg_s.A_y, marg_s.A_z,marg_s.G_x, marg_s.G_y, marg_s.G_z, marg_s.M_x, marg_s.M_y, marg_s.M_z);
		//delay_ms(100);
		
		
	}

	// Insert application code here, after the board has been initialized.
}
